#include <stdlib.h>
#include <stdio.h>
#include <unistd.h>
#include <fcntl.h>
#include <string.h>
#include <time.h>
#include <errno.h>
#include "common.h"
#include <pthread.h>

/* Global data */
_mt_vp_t _mt_this_vp[16];           /* This VP */
_mt_thread_t *_mt_this_thread[16];  /* Current thread */
int _mt_active_count[16] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};       /* Active thread count */
int _mt_IDs[16] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};

void mt_noop() {
	return;
}

void _mt_vp_schedule()
{
	uint32_t vp = mt_get_vcpu();
	if(vp == VCPU_ERROR)
		throw "err in vpshed!";
	
	bool needToContinue = false;
	_mt_thread_t *thread;
	//printf("switching from %u\n", _mt_this_thread[vp]->ID);
	
	while(!needToContinue) {
		thread = _mt_this_vp[vp].run_q = _mt_this_vp[vp].run_q->next;
		
		switch(thread->state) {
			case _MT_MT_RUNNABLE:
				thread->state = _MT_MT_RUNNING;			
			case _MT_MT_RUNNING:
				needToContinue = true;
				break;
			case _MT_MT_IO_WAIT:
				if(thread->waitRound++ > 128) {
					thread->waitRound = 0;
					thread->state = _MT_MT_RUNNING;
					needToContinue = true;
				}
				break;
			case _MT_MT_LOCK_WAIT:
				break;
			case _MT_MT_COND_WAIT:
				if(*(thread->desc.data) == thread->waitValue) {
					thread->state = _MT_MT_RUNNING;
					needToContinue = true;
				}
				break;
			case _MT_MT_SLEEPING:
				break;
			case _MT_MT_ZOMBIE:
				if(_mt_this_thread[vp] == thread) {
					//printf("avoiding active stack death\n");
					continue;
				}
								
				_mt_this_vp[vp].run_q = thread->next;
				thread->prev->next = thread->next;
				thread->next->prev = thread->prev;
				
				if (!(thread->flags & _MT_FL_PRIMORDIAL))
					_mt_stack_free(thread->stack);

				break;
			case _MT_MT_NOTCOND_WAIT:
				if(*(thread->desc.data) != thread->waitValue) {
					thread->state = _MT_MT_RUNNING;
					needToContinue = true;
				}
				break;
			default:
				break;
		}
	}
	_mt_this_thread[vp] = thread;	
	//printf("switching to %u\n", thread->ID);
	_MT_RESTORE_CONTEXT(thread, vp);
	mt_noop();
}


/*
 * Initialize this Virtual Processor
 */
void vp_destroy() {
	uint32_t vp = mt_get_vcpu();
	if(vp == VCPU_ERROR)
		throw "err in vpdestr!";
	
	//_mt_delete_stacks();
	free(_mt_this_vp[vp].prim);
	memset(&_mt_this_vp[vp], 0, sizeof(_mt_vp_t));
	_mt_active_count[vp] = 0;
	_mt_IDs[vp] = 0;
}
 
uint32_t vp_init(uint32_t vp)
{
	int retVal = -1;
	uint32_t vpCnt = vp;
	bool found = false;
	
	if(_mt_active_count[vpCnt] != 0)
		throw "cpu not halted";
	
	_mt_thread_t *thread;
	memset(&_mt_this_vp[vpCnt], 0, sizeof(_mt_vp_t));

	_mt_this_vp[vpCnt].pself = pthread_self();
	_mt_this_vp[vpCnt].pagesize = getpagesize();

	thread = (_mt_thread_t *) calloc(1, sizeof(_mt_thread_t) +	(MT_KEYS_MAX * sizeof(void *)));
	_mt_this_vp[vpCnt].prim = thread;
	
	if (!thread) return 0xffffffff;
	
	//thread->private_data = (void **) (thread + 1);
	thread->state = _MT_MT_RUNNING;
	thread->flags = _MT_FL_PRIMORDIAL;

	thread->ID = _mt_IDs[vpCnt]++;
	
	_mt_this_thread[vpCnt] = thread;
	_mt_active_count[vpCnt]++;
	_mt_this_vp[vpCnt].run_q = thread;
	thread->prev = thread->next = thread;
	return vpCnt;
}

uint32_t mt_get_vcpu() {
	uint32_t i;
	pthread_t s = pthread_self();
	
	for(i = 0; i < 16; i++) {
		if(_mt_this_vp[i].pself == s)
			return i;
	}
		
	return 0xffffffff;
}

void mt_join_all() {
	uint32_t vp = mt_get_vcpu();
	if(vp == VCPU_ERROR)
		throw "err in join all!";

	_mt_thread_t *thread = _mt_this_thread[vp];

	while(_mt_active_count[vp] > 1)	 {
 		_MT_SWITCH_CONTEXT(thread, vp);
		mt_noop();
	}
}

//void mt_thread_exit(void *retval, int vp)
void mt_thread_exit()
{
	uint32_t vp = mt_get_vcpu();
	if(vp == VCPU_ERROR)
		throw "err in thr exit!";
	
	
	_mt_thread_t *thread = _mt_this_thread[vp];
	thread->state = _MT_MT_ZOMBIE;
	_mt_active_count[vp]--;	
	
	//_mt_this_vp[vp].run_q = _mt_this_vp[vp].run_q->prev;
	//thread->prev->next = thread->next;
	//thread->next->prev = thread->prev;

	_MT_SWITCH_CONTEXT(thread, vp);
}

void _mt_thread_main()
{
	uint32_t vp = mt_get_vcpu();
	if(vp == VCPU_ERROR)
		throw "err in thmain!";

	
 	_mt_thread_t *thread = _mt_this_thread[vp];
	
	if(thread == NULL) 
		throw "START THREAD = NULL!";
	
	if(thread->start == NULL)
		throw "UNSTARTABLE PRIM THREAD!";
	
	(*thread->start)(thread->arg);
	mt_thread_exit();
}

_mt_thread_t *mt_thread_create(void (start)(void *), void *arg)
{
	uint32_t vp = mt_get_vcpu();
	if(vp == VCPU_ERROR)
		return NULL;
	
	
	uint32_t stk_size = 32*1024;
	//printf("\n");	
	_mt_thread_t *thread;
	_mt_stack_t *stack;
	void **ptds;
	char *sp;

	/* Adjust stack size */
	if (stk_size == 0)
		stk_size = MT_DEFAULT_STACK_SIZE;
	
	stk_size = ((stk_size + _MT_PAGE_SIZE(vp) - 1) / _MT_PAGE_SIZE(vp)) * _MT_PAGE_SIZE(vp);
	stack = _mt_stack_new(stk_size);
	if (!stack)
		return NULL;

	/* Allocate thread object and per-thread data off the stack */
	sp = stack->stk_top;
	sp = sp - (MT_KEYS_MAX * sizeof(void *));
	ptds = (void **) sp;
	sp = sp - sizeof(_mt_thread_t);
	thread = (_mt_thread_t *) sp;

	/* Make stack 64-byte aligned */
	if ((unsigned long)sp & 0x3f)
		sp = sp - ((unsigned long)sp & 0x3f);
	stack->sp = sp - _MT_STACK_PAD_SIZE;
	memset(thread, 0, sizeof(_mt_thread_t));
	memset(ptds, 0, MT_KEYS_MAX * sizeof(void *));

	/* Initialize thread */
	thread->private_data = ptds;
	thread->stack = stack;
	thread->start = start;
	thread->arg = arg;

	thread->ID = _mt_IDs[vp]++;
	
	/* Make thread runnable */
	thread->state = _MT_MT_RUNNABLE;
	_mt_active_count[vp]++;
	
	_MT_INIT_CONTEXT(thread, stack->sp, _mt_thread_main);
	
	thread->prev = _mt_this_vp[vp].run_q->prev;
	thread->next = _mt_this_vp[vp].run_q;
	thread->prev->next = thread;
	thread->next->prev = thread;
	
	return thread;
}

_mt_thread_t *mt_thread_self()
{
	uint32_t vp = mt_get_vcpu();
	if(vp == VCPU_ERROR)
		return NULL;

	return _MT_CURRENT_THREAD(vp);
}
